#include "stm32f10x_conf.h"
#include "motor.h"

/* Motor driver pins */
struct TMotorDriver
{
  /* PWM register */
  __IO uint16_t *CCR;
  /* PWM port and pin */
  GPIO_TypeDef *PWM_port;
  u32 PWM_pin;
  /* Direction 1 port and pin */
  GPIO_TypeDef *DIR1_port;
  u32 DIR1_pin;
  /* Direction 2 port and pin */
  GPIO_TypeDef *DIR2_port;
  u32 DIR2_pin;
}; 

/* Motor drivers definition */
struct TMotorDriver drivers[] = { 
  {&M1_PWM_REGISTER, M1_PWM_PORT, M1_PWM_PIN, M1_DIR1_PORT, M1_DIR1_PIN, M1_DIR2_PORT, M1_DIR2_PIN},
  {&M2_PWM_REGISTER, M2_PWM_PORT, M2_PWM_PIN, M2_DIR1_PORT, M2_DIR1_PIN, M2_DIR2_PORT, M2_DIR2_PIN},
  {&M3_PWM_REGISTER, M3_PWM_PORT, M3_PWM_PIN, M3_DIR1_PORT, M3_DIR1_PIN, M3_DIR2_PORT, M3_DIR2_PIN},
  {&M4_PWM_REGISTER, M4_PWM_PORT, M4_PWM_PIN, M4_DIR1_PORT, M4_DIR1_PIN, M4_DIR2_PORT, M4_DIR2_PIN} };

/* Enable motor */
void EnableMotor(u8 channel)
{
  assert_param(IS_VALID_MOTOR_CHANNEL(channel));
  
  GPIO_InitTypeDef GPIO_InitStructure;
  struct TMotorDriver *driver = &drivers[channel - 1];
  
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Pin = driver->PWM_pin;
  GPIO_Init(driver->PWM_port, &GPIO_InitStructure);
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  GPIO_InitStructure.GPIO_Pin = driver->DIR1_pin;
  GPIO_Init(driver->DIR1_port, &GPIO_InitStructure);  
  GPIO_InitStructure.GPIO_Pin = driver->DIR2_pin;
  GPIO_Init(driver->DIR2_port, &GPIO_InitStructure);
}

void StopMotor(u8 channel)
{
  assert_param(IS_VALID_MOTOR_CHANNEL(channel));
  
  struct TMotorDriver *driver = &drivers[channel - 1];
  GPIO_ResetBits(driver->DIR1_port, driver->DIR1_pin);
  GPIO_ResetBits(driver->DIR2_port, driver->DIR2_pin);
  *driver->CCR = 999;
}

/* Set speed of specified motor */
int SetSpeed(u8 channel, int speed)
{
  assert_param(IS_VALID_MOTOR_CHANNEL(channel));
  assert_param(IS_VALID_MOTOR_SPEED(speed));
  
  struct TMotorDriver *driver = &drivers[channel - 1];
  /* Set direction pins according to speed sign */
  if(speed < 0)
  {
    speed = -speed;
    GPIO_ResetBits(driver->DIR1_port, driver->DIR1_pin);
    GPIO_SetBits(driver->DIR2_port, driver->DIR2_pin);
  }
  else
  {
    if(speed == 0)
    {
      speed = 1000;
      GPIO_ResetBits(driver->DIR1_port, driver->DIR1_pin);
    }
    else
      GPIO_SetBits(driver->DIR1_port, driver->DIR1_pin);
    GPIO_ResetBits(driver->DIR2_port, driver->DIR2_pin);
  }
  /* Set PWM */
  *driver->CCR = speed;
  
  return speed;
}

/* Get speed of specified motor */
int GetSpeed(u8 channel)
{
  assert_param(IS_VALID_MOTOR_CHANNEL(channel));
  
  struct TMotorDriver *driver = &drivers[channel - 1];
  /* Get PWM */
  int speed = *driver->CCR;
  /* Set speed sign according to dir pins*/
  if(GPIO_ReadInputDataBit(driver->DIR2_port, driver->DIR2_pin) 
     && !GPIO_ReadInputDataBit(driver->DIR1_port, driver->DIR1_pin))
    speed = -speed;
  /* If PWM is enabled, but dir pins is in break position, speed=0 */
  else if(!GPIO_ReadInputDataBit(driver->DIR2_port, driver->DIR2_pin) 
     && !GPIO_ReadInputDataBit(driver->DIR1_port, driver->DIR1_pin))
    speed = 0;
  
  return speed;
}

/* Read ADC1 channel */
u16 readADC1(u8 channel)
{
  assert_param(IS_VALID_ADC_CHANNEL(channel));
  channel = START_ADC_CHANNEL - 1 + channel;
  
  ADC_RegularChannelConfig(ADC1, channel, 1, ADC_SampleTime_13Cycles5);
  // Start the conversion
  ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  // Wait until conversion completion
  while(ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == RESET);
  // Get the conversion value
  return ADC_GetConversionValue(ADC1);
}
